#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <cre/CreActionAction.h>

class CreActionServer {
protected:
    ros::NodeHandle nh_;
    actionlib::SimpleActionServer<cre::CreActionAction> as_;
    std::string action_name_;
    cre::CreActionFeedback feedback_;
    cre::CreActionResult result_;

public:
    CreActionServer(std::string name) :
            as_(nh_, name, boost::bind(&CreActionServer::executeCB, this, _1), false),
            action_name_(name) {
        as_.start();
    }

    ~CreActionServer(void) {}

    void executeCB(const cre::CreActionGoalConstPtr &goal) {
        ros::Rate r(1);
        bool success = true;

        // 执行动作
        for (int i = 1; i <= 10; i++) {
            if (as_.isPreemptRequested() || !ros::ok()) {
                ROS_INFO("%s: Preempted", action_name_.c_str());
                as_.setPreempted();
                success = false;
                break;
            }

            feedback_.progress = i * 10;
            ROS_INFO("%s: Executing, progress = %f", action_name_.c_str(), feedback_.progress);
            as_.publishFeedback(feedback_);

            r.sleep();
        }

        // 发送结果
        if (success) {
            result_.success = true;
            ROS_INFO("%s: Succeeded", action_name_.c_str());
            as_.setSucceeded(result_);
        }
    }
};

int main(int argc, char **argv) {
    ros::init(argc, argv, "cre_server");
    CreActionServer server("cre_action");
    ros::spin();
    return 0;
}